#include "InterpolateLinear.h"

CvScalar InterpolateLinear::interpolate (IplImage *image, CvPoint2D32f p)
{
	try {
  int w = image->width;
  int h = image->height;
  if (p.x < 0 || p.x > w - 1 || p.y < 0 || p.y > h - 1)
    return cvScalar (0);
  if (p.x == w - 1 || p.y == h - 1)
    return cvGet2D (image, p.y, p.x);

  CvPoint p0 = cvPoint ((int)p.x, (int)p.y);
  CvPoint p1 = cvPoint ((int)p.x + 1, (int)p.y);
  CvPoint p2 = cvPoint ((int)p.x + 1, (int)p.y + 1);
  CvPoint p3 = cvPoint ((int)p.x, (int)p.y + 1);

  CvScalar colorRet;
  CvScalar rgb0 = cvGet2D (image, p0.y, p0.x);
  CvScalar rgb1 = cvGet2D (image, p1.y, p1.x);
  CvScalar rgb2 = cvGet2D (image, p2.y, p2.x);
  CvScalar rgb3 = cvGet2D (image, p3.y, p3.x);

  for (int i = 0; i < image->nChannels; ++i)
  {
    float c0 = rgb0.val[i];
    float c1 = rgb1.val[i];
    float c2 = rgb2.val[i];
    float c3 = rgb3.val[i];
    float t0 = (c1 - c0) * (p.x - p0.x) + c0;
    float t1 = (c2 - c3) * (p.x - p0.x) + c3;
    colorRet.val[i] = (t1 - t0) * (p.y - p0.y) + t0;
  }

  return colorRet;
	} catch (...) {
		int a = 0;
	}

}
